/**
@brief A demo to spin motors in same speed synchronally by pos control.
*/

#include "board.h"
#include "mb.h"
#include "MotorControl.h"


#define TIM_FREQ  10        // Hz, timer's frequency
#define LOOPS     10000      // d3 format, spin how many loops

int g_dir = 1;                // direction of spinning
int32_t g_s32d3Speed = 1200;  // speed, revs/sec


static SoftTimer_TypeDef g_tim;



// called periodically
void calc_pos(void) {
    int32_t _dt_ms = 1000/TIM_FREQ;
    
    if (
          (M1.ParamRW.s32d3PositionTar > LOOPS) 
        ||(M1.ParamRW.s32d3PositionTar < -LOOPS)
       ) 
    {
        g_dir *= -1;  // change direction
    }
    
    M1.ParamRW.s32d3PositionTar += g_dir * _dt_ms * g_s32d3Speed / 1000;
    M2.ParamRW.s32d3PositionTar = M1.ParamRW.s32d3PositionTar;
    M3.ParamRW.s32d3PositionTar = M1.ParamRW.s32d3PositionTar;
    M4.ParamRW.s32d3PositionTar = M1.ParamRW.s32d3PositionTar;
}


void demo_sync_spin(void) {
    eMBInit(MB_RTU, 0x01, 0x00, 9600, MB_PAR_NONE);
    eMBEnable();
    
    SfTim_Init(&g_tim, 100e3/TIM_FREQ, calc_pos);
    Mot_InitSys();
    
    M1.ParamRW.u16MotCtrlMod = MotCtrlMod_PosPid;
    M2.ParamRW.u16MotCtrlMod = MotCtrlMod_PosPid;
    M3.ParamRW.u16MotCtrlMod = MotCtrlMod_PosPid;
    M4.ParamRW.u16MotCtrlMod = MotCtrlMod_PosPid;
    
    M1.ParamRW.s16ReductionRatio = 20;

    M1.pstHBdg->ParamChanged = true;
    M1.pstHBdg->Param.State = HB_State_Run;
    M1.pstHBdg->Param.DirMode = HB_DirMode_ALBHasForward;
    M1.pstHBdg->Param.Dir = HB_Dir_Backward;
    
    M1.pstEnc->ParamChanged = true;
    M1.pstEnc->Param.DirMode = Enc_DirMode_0;
    M1.pstEnc->Param.PulsPerRev = 13;
    
    M2.ParamRW.s16ReductionRatio = 35;

    M2.pstHBdg->ParamChanged = true;
    M2.pstHBdg->Param.State = HB_State_Run;
    M2.pstHBdg->Param.DirMode = HB_DirMode_ALBHasForward;
    M2.pstHBdg->Param.Dir = HB_Dir_Backward;
    
    M2.pstEnc->ParamChanged = true;
    M2.pstEnc->Param.DirMode = Enc_DirMode_0;
    M2.pstEnc->Param.PulsPerRev = 11;
    
    M3.ParamRW.s16ReductionRatio = 150;

    M3.pstHBdg->ParamChanged = true;
    M3.pstHBdg->Param.State = HB_State_Run;
    M3.pstHBdg->Param.DirMode = HB_DirMode_AHBLasForward;
    M3.pstHBdg->Param.Dir = HB_Dir_Backward;
    
    M3.pstEnc->ParamChanged = true;
    M3.pstEnc->Param.DirMode = Enc_DirMode_1;
    M3.pstEnc->Param.PulsPerRev = 7;
    
    M4.ParamRW.s16ReductionRatio = 48;

    M4.pstHBdg->ParamChanged = true;
    M4.pstHBdg->Param.State = HB_State_Run;
    M4.pstHBdg->Param.DirMode = HB_DirMode_ALBHasForward;
    M4.pstHBdg->Param.Dir = HB_Dir_Backward;
    
    M4.pstEnc->ParamChanged = true;
    M4.pstEnc->Param.DirMode = Enc_DirMode_1;
    M4.pstEnc->Param.PulsPerRev = 13;
    
    while (1) {
        eMBPoll();
        
        Mot_Update(&M1);
        Mot_Update(&M2);
        Mot_Update(&M3);
        Mot_Update(&M4);
    } 
}

